#!/usr/bin/env python3
# encoding: utf-8

import rospy
from std_msgs.msg import String
import binascii
import serial

ser1_params = {
    'port': '/dev/ttyUSB0',  # 发送指令口
    'baudrate': 115200,
    'timeout': 1
}

class VehicleController:
    def __init__(self):
        rospy.init_node('vehicle_controller', anonymous=True)

        # 初始化串口
        try:
            self.ser1 = serial.Serial(**ser1_params)
        except serial.SerialException as e:
            rospy.logerr("无法打开串口: {}".format(e))
            rospy.signal_shutdown("串口打开失败")

        if not self.ser1.isOpen():
            rospy.logerr("串口未成功打开")
            rospy.signal_shutdown("串口未成功打开")

        self.command_subscriber = rospy.Subscriber('vehicle_command', String, self.command_callback)
        self.command_publisher = rospy.Publisher('vehicle_status', String, queue_size=10)  # 发布车辆状态

    def command_callback(self, msg):
        command = msg.data
        if command == "enable_first_channel":
            self.enable_first_channel()
        elif command == "stop":
            self.stop()
        elif command == "move_forward":
            self.move_forward(speed=-1000, angle=0)  # 示例速度和角度
        elif command == "move_backward":
            self.move_backward(speed=1000, angle=0)  # 示例速度和角度
        elif command == "turn_left":
            self.turn_left(angle=-150)  # 示例角度
        elif command == "turn_right":
            self.turn_right(angle=150)  # 示例角度
        else:
            rospy.loginfo("未知命令: {}".format(command))  # 使用 .format() 方法替换 f-string

    def stop(self):
        rospy.loginfo("车辆已停止。")
        # 发送停止命令
        self.send_command("E00100000000000000000000")

    def move_forward(self, speed, angle):
        rospy.loginfo("车辆前进。")
        # 如果速度是负数，将其转换为无符号整数
        if speed < 0:
            speed = (1 << 32) + speed  # 转换为无符号整数
        # 发送前进命令，构造命令字符串        
        command = f"E0010000{angle:08X}{speed:08X}"
        self.send_command(command)

    def move_backward(self, speed, angle):
        rospy.loginfo("车辆后退。")
        if speed < 0:
            speed = (1 << 32) + speed  # 转换为无符号整数
        
        # 发送后退命令，构造命令字符串
        command = f"E0010000{angle:08X}{speed:08X}"
        self.send_command(command)

    def turn_left(self, angle):
        rospy.loginfo("机器人向左转")
        # 发送左转命令，构造命令字符串
        command = f"E0010000{angle:08X}00000000"
        self.send_command(command)

    def turn_right(self, angle):
        rospy.loginfo("机器人向右转")
        if angle < 0:
            angle = (1 << 32) + angle  # 转换为无符号整数
        
        # 发送右转命令，构造命令字符串
        command = f"E0010000{angle:08X}00000000"
        self.send_command(command)

    def send_command(self, command):
        try:
            byte_command = binascii.unhexlify(command)
            self.ser1.write(byte_command)
            rospy.loginfo("发送命令: {}".format(command))
            self.command_publisher.publish(command)
        except Exception as e:
            rospy.logerr("发送命令失败: {}".format(e))

    def run(self):
        rospy.spin()

    def __del__(self):
        if self.ser1.isOpen():
            self.ser1.close()

if __name__ == '__main__':
    try:
        controller = VehicleController()
        controller.run()
    except rospy.ROSInterruptException:
        pass